classdef AttDiffTest < matlab.unittest.TestCase
    %ATTDIFFTEST DCM与四元数比较函数测试
    
    properties
        NSamp
        RotAngleMaxMag
        RotAngleErrMaxMag
    end
    
    methods(TestClassSetup)
        function createInput(testCase)
            testCase.NSamp = 1000;
            testCase.RotAngleMaxMag = pi; % rad
            testCase.RotAngleErrMaxMag = 1; % deg
        end
    end
    
    methods(Test)
        function vldDCMDiffQuatDiff(testCase)
            r = rand(testCase.NSamp, 3) * 2*testCase.RotAngleMaxMag - testCase.RotAngleMaxMag;
            dr = (rand(testCase.NSamp, 3)*2*testCase.RotAngleErrMaxMag-testCase.RotAngleErrMaxMag) / 180 * pi;
            CFM = angle2dcm(r(:, 1), r(:, 2), r(:, 3));
            CHatFM = angle2dcm(r(:, 1)+dr(:, 1), r(:, 2)+dr(:, 2), r(:, 3)+dr(:, 3));
            betaFMM = dcmdiff(CHatFM, CFM);
            betaFMM0 = dcmdiff(CHatFM, CFM, false);
            dDCMDiffAbs = logtestdatastat(testCase, abs(betaFMM-betaFMM0), '一阶近似与精确betaFMM差值绝对值（rad）');
            testCase.verifyLessThanOrEqual(dDCMDiffAbs, 1e-5);
            
            qMF = dcm2quat_cg(CFM);
            qHatMF = dcm2quat_cg(CHatFM);
            [betaMFF, q0] = quatdiff_mult(qHatMF, qMF);
            betaMFF0 = quatdiff_mult(qHatMF, qMF, false);
            dQuatDiffAbs = logtestdatastat(testCase, abs(betaMFF-betaMFF0), '一阶近似与精确betaMFF差值绝对值（rad）');
            testCase.verifyLessThanOrEqual(dQuatDiffAbs, 1e-5);
            testCase.verifyLessThanOrEqual(logtestdatastat(testCase, abs(q0), '一阶近似-2dqMF°(qMF*)的标量部分绝对值'), 5e-4);
            
            dDiffAbs_exact_CFM = logtestdatastat(testCase, abs(binopalonglastdim(CFM, betaMFF0')'+betaFMM0), 'betaMFF0与betaFMM0差值绝对值（使用CFM转换，rad）');
            testCase.verifyLessThanOrEqual(dDiffAbs_exact_CFM, 1e-15);
            dDiffAbs_exact_CHatFM = logtestdatastat(testCase, abs(binopalonglastdim(CHatFM, betaMFF0')'+betaFMM0), 'betaMFF0与betaFMM0差值绝对值（使用CHatFM转换，rad）');
            testCase.verifyLessThanOrEqual(dDiffAbs_exact_CHatFM, 1e-15);
        end
    end
end